import launch
from launch.substitutions import Command, LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import launch_ros
from launch.actions import TimerAction
import os

def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='agv_bringup').find('agv_bringup')
    ################### user configure parameters for ros2 start ###################
    # xfer_format   = 0    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
    xfer_format   = 4    # 2023.10.11修改
    multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
    data_src      = 0    # 0-lidar, others-Invalid data src
    publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
    output_type   = 0
    frame_id      = 'base_link'
    lvx_file_path = '/home/livox/livox_test.lvx'
    cmdline_bd_code = 'livox0000000001'
    user_config_path = os.path.join(pkg_share, 'config', 'MID360_config.json')
    # rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz')
    ################### user configure parameters for ros2 end #####################
    livox_ros2_params = [
        {"xfer_format": xfer_format},
        {"multi_topic": multi_topic},
        {"data_src": data_src},
        {"publish_freq": publish_freq},
        {"output_data_type": output_type},
        {"frame_id": frame_id},
        {"lvx_file_path": lvx_file_path},
        {"user_config_path": user_config_path},
        {"cmdline_input_bd_code": cmdline_bd_code}
    ]

    livox_driver = launch_ros.actions.Node(
        package='livox_ros_driver2',
        executable='livox_ros_driver2_node',
        name='livox_lidar_publisher',
        output='log',
        parameters=livox_ros2_params
        )
    
    fast_lio2_node = launch_ros.actions.Node(
        package='fast_lio',
        executable='fastlio_mapping',
        parameters=[os.path.join(pkg_share, 'config', 'mid360.yaml'),],
        output='log'
    )

    return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument(name='map_path', default_value='my_map.pgm',
                                            description='Absolute path to map file'),
        livox_driver,
        fast_lio2_node,
        
    ])
